/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package dynamixel;

import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.io.Serializable;
import java.util.Arrays;
import java.util.logging.Level;
import java.util.logging.Logger;
import javax.swing.Timer;
import jssc.SerialPort;
import jssc.SerialPortEvent;
import jssc.SerialPortEventListener;
import jssc.SerialPortException;
import jssc.SerialPortList;
import jssc.SerialPortTimeoutException;

/**
 *
 * @author Asko
 */
public class HostActuator implements Serializable {

    final static int[] SEND_PACK = {0xff, 0xff, 0x01, 0x02, 0x01, 0xfb};
    static int[] CHECK_PACK = {0xff, 0xff, 0x01, 0x02, 0x00, 0xfc};
    final static int BYTES_CNT = 6;
    final static int BAUD_RATE_DYN = 57142;
    final static int TIMEOUT = 100;
    private boolean lastOperationSuccess = false;
    private boolean isTicked = false;
    private static SerialPort serialport;
    private int[] currentPacket;
    private boolean isValid = true;
    private int delayTime = 5000;
    static int[] resultBuf = new int[6];
    static int[] statusPacket = new int[6];
    static int[] buffer = new int[10];
    int[] lastMessage = new int[1024];
    int[] inputBuf;
    int bytesCnt = 0;
    private Timer timer = new Timer(delayTime, new ActionListener() {
        @Override
        public void actionPerformed(ActionEvent e) {
            System.out.println("Packet is missed");
            isTicked = true;
            lastOperationSuccess = false;
            setValidOn();
        }
    });
    
    public HostActuator(int baudRate) {
        this(ComPortFinder.findCom(new ComPortFindParam(BAUD_RATE_DYN, SerialPort.DATABITS_8, SerialPort.STOPBITS_1,
                SerialPort.PARITY_NONE,  BYTES_CNT, TIMEOUT) {
            @Override
            public boolean isFinded(int[] pckg) {
                CHECK_PACK[4] = pckg[4];
                CHECK_PACK[5] = Actuator.getCheckSum(CHECK_PACK);
                
                return Arrays.equals(pckg, CHECK_PACK);
            }

            @Override
            public int[] getSendPckg() {
                return SEND_PACK;
            }
        }), baudRate);
    }
    public HostActuator(String port, int baudRate) {
        System.out.println("Initialyzing HostActuator: " + port);
        if(port == null){
            System.exit(1);
        }
        serialport = new SerialPort(port);
        try {
            serialport.openPort();

            serialport.setParams(baudRate, 8, 1, 0);
            serialport.addEventListener(new SerialPortEventListener() {
                @Override
                public void serialEvent(SerialPortEvent serialPortEvent) {
                    setValidOff();
                    try {
                        //  System.out.println("Res " + serialport.readHexString() );
                        inputBuf = serialport.readIntArray();
                        if (inputBuf != null) {
                            if (inputBuf.length > 1) {
                                if (inputBuf[0] == 0xff && inputBuf[1] == 0xff) {
                                    System.arraycopy(inputBuf, 0, lastMessage, 0,
                                            inputBuf.length);
                                    bytesCnt = inputBuf.length;
                                    //setMsgCnt(inputBuf.length);
                                } else {
                                    System.arraycopy(inputBuf, 0, lastMessage, bytesCnt,
                                            inputBuf.length);
                                    bytesCnt += inputBuf.length;
                                    //setMsgCnt(bytesCnt + inputBuf.length);
                                }
                            } else {
                                System.arraycopy(inputBuf, 0, lastMessage, bytesCnt,
                                        inputBuf.length);
                            }

                            // System.out.println("Value : " + serialport.readHexString());
                        }
                    } catch (SerialPortException ex) {
                        Logger.getLogger(HostActuator.class.getName()).
                                log(Level.SEVERE, null, ex);
                    }
                    setValidOn();
                }
            });
        } catch (SerialPortException ex) {
            Logger.getLogger(HostActuator.class.getName()).log(Level.SEVERE, null, ex);
        }
    }
    /*This command does not instruct anything. It is only used when receiving 
     * Status Packet or confirming the existence of RX-64 with a specific ID*/
    public static final int PING = 0x01;
    /*This command is to read data in the Control Table inside of RX-64*/
    public static final int READ_DATA = 0x02;
    /*This command is to write data to the Control Table inside of RX-64*/
    public static final int WRITE_DATA = 0x03;
    /* The REG_WRITE command is similar to the WRITE_DATA command in terms of 
     * function, but differs in terms of the timing that a command is executed. 
     * When Instruction Packet arrives, it is saved in Buffer and the Write 
     * operation remains in the standby state. At this moment, Registered 
     * Instruction (Address 44 (0x2C)) is set as “1”. Then, when Action 
     * Instruction Packet arrives, Registered Instruction changes into “‘0” 
     * and the registered Write command is finally executed*/
    public static final int REG_WRITE = 0x04;
    /*This command is to execute the Write action registered by REG_WRITE*/
    public static final int ACTION = 0x05;
    /*This command is to reset the Control Table of RX-64 to the factory default setting*/
    public static final int RESET = 0x06;
    /* This command is used to control several RX-64s simultaneously with one 
     * Instruction Packet transmission. When this command is used, several 
     * commands are transmitted at once, so that the communication time is 
     * reduced when multiple RX-64s are controlled. However, the SYNC WRITE 
     * command can be used only if both of the address and length of the Control
     * Table to write is identical. Besides, ID should be transmitted as 
     * Broadcasting ID.*/
    public static final int SYNC_WRITE = 0x07;
    public static final int ENGINES_NUM = 1;
    public static final int BROADCAST_ID = 0xfe;
    /*Адреса в таблице управления*/
    /**
     * EEPROM*
     */
    public static final short MODEL_NUMBER_L = 0x00;
    public static final short MODEL_NUMBER_H = 0x01;
    public static final short VERSION_OF_FIRMWARE = 0x02;
    public static final short ID = 0x03;
    public static final short MODEL_NUMBER = 0x00;
    public static final short BAUD_RATE = 0x04;
    public static final short RETURN_DELAY_TIME = 0x05;
    public static final short CW_ANGLE_LIMIT_L = 0x06;
    public static final short CW_ANGLE_LIMIT_H = 0x07;
    public static final short CCW_ANGLE_LIMIT_L = 0x08;
    public static final short CCW_ANGLE_LIMIT_H = 0x09;
    public static final short HIGH_LIMIT_TEMPER = 0x0b;
    public static final short LOWEST_LIMIT_VOLTAGE = 0x0c;
    public static final short HIGHEST_LIMIT_VOLTAGE = 0x0D;
    public static final short MAX_TORQUE_L = 0x0e;
    public static final short MAX_TORQUE_H = 0x0f;
    public static final short STATUS_RETURN_LEVEL = 0x10;
    public static final short ALARM_LED = 0x11;
    public static final short ALARM_SHUTDOWN = 0x12;
    /**
     * *************RAM*****************
     */
    public static final short TORQUE_ENABLE = 0x18;
    public static final short LED = 0x19;
    public static final short D_GAIN = 0x1a;
    public static final short I_GAIN = 0x1b;
    public static final short P_GAIN = 0x1c;
    public static final short GOAL_POSITION_L = 0x1e;
    public static final short GOAL_POSITION_H = 0x1f;
    public static final short MOVING_SPEED_L = 0x20;
    public static final short MOVING_SPEED_H = 0x21;
    public static final short TORQUE_LIMIT_L = 0x22;
    public static final short TORQUE_LIMIT_H = 0x23;
    public static final short PRESENT_POSITION_L = 0x24;
    public static final short PRESENT_POSITION_H = 0x25;
    public static final short PRESENT_SPEED_L = 0x26;
    public static final short PRESENT_SPEED_H = 0x27;
    public static final short PRESENT_LOAD_L = 0x28;
    public static final short PRESENT_LOAD_H = 0x29;
    public static final short PRESENT_VOLTAGE = 0x2a;
    public static final short PRESENT_TEMPERATURE = 0x2b;
    public static final short REGISTERED = 0x2c;
    public static final short MOVING = 0x2e;
    public static final short LOCK = 0x2f;
    public static final short PUNCH_L = 0x30;
    public static final short PUNCH_H = 0x31;
    public static final short CURRENT_L = 0x44;
    public static final short CURRENT_H = 0x45;
    public static final short TORQUE_CONTROL_MODE_ENABLE = 0x46;
    public static final short GOAL_TORQUE_L = 0x47;
    public static final short GOAL_TORQUE_H = 0x48;
    public static final short GOAL_ACCELERATION = 0x49;
    ///////////////////////////////
    public static final short INPUT_VOLTAGE_ERROR = 1;
    public static final short ANGLE_LIMIT_ERROR = 2;
    public static final short OVERHEATING_ERROR = 4;
    public static final short RANGE_ERROR = 8;
    public static final short CHECKSUM_ERROR = 16;
    public static final short OVERLOAD_ERROR = 32;
    public static final short INSTRUCTION_ERROR = 64;

    public void closePort() {
        try {
            while (!isValid) {
            };
            serialport.closePort();
        } catch (SerialPortException ex) {
            Logger.getLogger(HostActuator.class.getName()).log(Level.SEVERE, null, ex);
        }
    }

    @SuppressWarnings("empty-statement")
    public synchronized void sendPacket(int[] pack) {
        try {
            lastOperationSuccess = false;
            if (!serialport.isOpened()) {
                serialport.openPort();
            }
            currentPacket = pack;
            Thread ct = new Thread(new Runnable() {
                @Override
                public void run() {
                    try {
                        while (!isValid) {
                        }
                        isValid = false;
                        serialport.writeIntArray(currentPacket);
                        setValidOff();
                    } catch (SerialPortException ex) {
                        Logger.getLogger(HostActuator.class.getName()).
                                log(Level.SEVERE, null, ex);
                    }
                }
            });
            ct.start();
            long i = 1000000000;
            while(ct.isAlive() && i<0){
                i--;
            }
            isValid = true;
            try {
                Thread.sleep(10);
            } catch (InterruptedException ex) {
                Logger.getLogger(HostActuator.class.getName()).log(Level.SEVERE, null, ex);
            }
        } catch (SerialPortException ex) {
            Logger.getLogger(HostActuator.class.getName()).
                    log(Level.SEVERE, null, ex);
        }
    }

    private void setValidOn() {
        isValid = true;
        timer.stop();
        lastOperationSuccess = !isTicked;
        isTicked = false;
    }

    public synchronized int getMsgCnt() {
        return bytesCnt;
    }

    private void setMsgCnt(int size) {
        bytesCnt = size;
    }

    public boolean isLastOperationSuccess() {
        return lastOperationSuccess;
    }

    private void setValidOff() {
        isValid = false;
        timer.start();
    }

    public int[] getLastMessage() {
        int[] cnt = new int[bytesCnt];
        System.arraycopy(lastMessage, 0, cnt, 0, cnt.length);
        return cnt;
    }

    public int getLastMesID() {
        return lastMessage[2];
    }

    public boolean isErrorInLastMsg() {
        return lastMessage[4] != 0;
    }

    public synchronized void flushBytesCnt() {
        bytesCnt = 0;
    }

    public int getErrorInlastMsg() {
        return lastMessage[4];
    }
}
